
"""
PyQt6 主窗口 + 绘图
飞机类在 aircraft.py
"""

from PyQt6.QtWidgets import QApplication, QMainWindow, QLabel, QVBoxLayout, QWidget
from PyQt6.QtCore import Qt, QTimer, QPointF
from PyQt6.QtGui import QPainter, QPen, QBrush, QColor, QPolygonF, QTransform

#自己写的包
from params import DT, SCALE, CENTER_X, CENTER_Y, N ,V_LOOP
from params import x_nose, y_le, x_tail, y_te

from function import  generate_formation_poses, plan_path
from aircraft import Aircraft


class MainWindow(QMainWindow):
    def __init__(self):
        super().__init__()
        self.setWindowTitle("FormationDrone")
        self.resize(1400, 1000)#窗口大小
        
        self.label = QLabel()
        self.label.setAlignment(Qt.AlignmentFlag.AlignLeft | Qt.AlignmentFlag.AlignTop)#对齐方式，左对齐，顶部对齐，左上角
        central = QWidget()
        lay = QVBoxLayout(central)
        lay.addWidget(self.label)
        self.setCentralWidget(central)#创建一个中心控件，并将其设置为中央区域
        
        
        self.label_info = QLabel("目标区域集结", self)
        self.label_info.setAlignment(Qt.AlignmentFlag.AlignCenter)
        self.label_info.setStyleSheet(
            "QLabel{ background:rgba(0,0,0,200); color:#00FF00; "
            "font:bold 24px; border-radius:10px; padding:10px; }"
        )
        self.label_info.adjustSize()
        self.label_info.hide()    #默认隐藏

        self.uavs = [Aircraft(i) for i in range(N)]     # 创建 N 架无人机
        self.targets = []
        self.activated = False


        #定时器，每0.05*1000=50ms
        self.timer = QTimer(self)
        self.timer.timeout.connect(self.update_sim)
        self.timer.start(int(DT * 1000))

    # ---------- 5 s 触发 ----------
    def trigger_formation(self):
        if self.activated:
            return #如果已经激活，就返回
        self.activated = True
        # 按 params 里形状生成目标位置
        self.targets = generate_formation_poses(
            center=(0, 0), r=80, n=N)
        max_L = 0.0
        for uav, tgt in zip(self.uavs, self.targets):
            p0, phi0 = (uav.x, uav.y), uav.phi
            path, L = plan_path(p0, tgt)#计算，离散直线，路径长度
            uav.path, uav.L = path, L
            max_L = max(max_L, L)
            
        #用最长时间的来计算时间
        #若用最短时间，则提高无人机的速度。
        # 同时到达：各自速度 = 自身长度 / (最长长度 / V_loop)
        T_arrive = max_L / V_LOOP
        for uav in self.uavs:
            uav.v = uav.L / T_arrive
            uav.phase = 1#标志位，开始直飞
            uav.s = 0.0
            
        # ===== 底部中央显示标签 =====
        self.label_info.move(
            (self.width() - self.label_info.width()) // 2,
            self.height() - self.label_info.height() - 30
        )
        self.label_info.show()
        #2s后隐藏信息
        QTimer.singleShot(2000, self.label_info.hide)
        
        

    # ---------- 仿真节拍 ----------
    def update_sim(self):
        t = self.uavs[0].t
        #前5s进行盘旋，50ms的间隔，在100ms内突然生成三个目标值
        if 4.95 < t < 5.05 and not self.activated:
            self.trigger_formation()
        for uav in self.uavs:
            uav.step(DT)
        self.draw()

    # ---------- 绘图 ----------
    def draw(self):
        '''画布->画笔->画图'''
        #如果还没创建过画布，就重新抓一张空白图。
        if not self.label.pixmap() or self.label.pixmap().size() != self.label.size():
            self.label.setPixmap(self.label.grab())#
        pm = self.label.pixmap()#获取画布
        painter = QPainter(pm)#创建一个画笔
        painter.fillRect(pm.rect(), QColor(12, 20, 40))#填充画布，颜色为深蓝色

        pen_grid = QPen(QColor(120, 120, 120), 1, Qt.PenStyle.DotLine)#设置网格线
        pen_loiter = QPen(QColor(180, 180, 180), 1, Qt.PenStyle.DashLine)#设置盘旋轨迹
        pen_path = QPen(QColor(0, 200, 255), 2)#设置航线
        pen_uav = [QPen(QColor(255, 80 + i * 35, 0), 3) for i in range(N)]#设置无人机

        # 画网格
        painter.setPen(pen_grid)
        for d in range(-1000, 1001, 200):
            xx = CENTER_X + d * SCALE
            yy = CENTER_Y - d * SCALE
            painter.drawLine(int(xx), 0, int(xx), 1000)
            painter.drawLine(0, int(yy), 1400, int(yy))


        # 盘旋轨迹
        if not self.activated:#航线激活
            for uav in self.uavs:
                #小于两个点 
                if len(uav.loiter_trail) < 2:
                    continue
                painter.setPen(pen_loiter)
                for j in range(1, len(uav.loiter_trail)):
                    a, b = uav.loiter_trail[j - 1], uav.loiter_trail[j]#取相邻两个点
                    ax, ay = CENTER_X + a[0] * SCALE, CENTER_Y - a[1] * SCALE#将点转换为像素坐标，屏幕y轴是向下的
                    bx, by = CENTER_X + b[0] * SCALE, CENTER_Y - b[1] * SCALE
                    painter.drawLine(int(ax), int(ay), int(bx), int(by))

        # 画航线
        if self.activated:#航线没激活
            painter.setPen(pen_path)
            for uav in self.uavs:
                if len(uav.path) > 1:
                    for i in range(len(uav.path) - 1):
                        a, b = uav.path[i], uav.path[i + 1]
                        ax, ay = CENTER_X + a[0] * SCALE, CENTER_Y - a[1] * SCALE
                        bx, by = CENTER_X + b[0] * SCALE, CENTER_Y - b[1] * SCALE
                        painter.drawLine(int(ax), int(ay), int(bx), int(by))

        # 画目标点
        if self.activated:
            painter.setPen(QPen(QColor(255, 220, 0), 8))
            for tx, ty in self.targets:
                x = CENTER_X + tx * SCALE
                y = CENTER_Y - ty * SCALE
                painter.drawPoint(int(x), int(y))

        
        
        #  画飞机 https://www.bilibili.com/opus/854876466763005960
        #https://www.desmos.com/calculator/eewdzlpbrt?lang=zh-CN
        def aircraft_poly( x_nose, x_tail, y_le, y_te):
            '''返回坐标位置'''
            return QPolygonF([
                QPointF(x_nose, 0),                         # 0 机头
                QPointF(x_nose*0.7,  y_le),                 # 1 右翼尖前缘
                QPointF(x_nose*0.7,  y_te),                 # 2 右翼尖后缘
                QPointF(x_tail, 0),                         # 3 机尾
                QPointF(x_nose*0.7, -y_te),                 # 4 左翼尖后缘
                QPointF(x_nose*0.7, -y_le),                 # 5 左翼尖前缘
                QPointF(x_nose, 0)                          # 6 闭合
            ])
            
        for i, uav in enumerate(self.uavs):
            painter.setPen(pen_uav[i])
            x = CENTER_X + uav.x * SCALE
            y = CENTER_Y - uav.y * SCALE
            phi = uav.phi

            #生成局部坐标系下的飞机外形
            poly = aircraft_poly(x_nose, x_tail, y_le, y_te)

            # 旋转 + 平移到屏幕坐标
            transform = QTransform()
            transform.translate(x, y)          # 飞机位置
            transform.rotateRadians(-phi)      # 负号：Qt 顺时针为正
            poly_tx = transform.map(poly)      # 一次性变换全部顶点
            painter.setBrush(QBrush(pen_uav[i].color()))
            painter.drawPolygon(poly_tx)
        painter.end()
        self.label.setPixmap(pm)